#pragma once

#include "BearingOnlyParticleFilter.h"
#include "LandmarkMap.h"
#include "ObservationImageLandmarks.h"

#include <fvision/Camera1D.h>

#include <mrpt/core.h>

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::utils;
using namespace std;
using namespace fvision;

class SimulationManager {
public:
	SimulationManager(void);
public:
	~SimulationManager(void);

public:
	void setNumParticles(int n) {
		numParticles = n;
	}

	void setDeltaTime(float t) { this->deltaTime = t; }

	void setInitialVehicleData(const CParticleVehicleData& data) { this->initialVehicleData = data; }

	void setVehicleDataStdDev(const CParticleVehicleData& stdDev) { this->stdDev = stdDev; }

	void setLandmarkMap(const LandmarkMap& landmarkMap) { this->landmarkMap = landmarkMap; }

	void setCamera1DInternal(const Camera1DInternal& camInternal) { this->camInternal = camInternal; }

	void run();

private:
	void setUpWindow();

	void initializeParticleFilter();

	void initializeGroundTruthVehicleData();

	void updateGroundTruthVehicleData();

	void simulateGroundTruthObservation();

	void simulateNoisyObservation();

	void processWithPF();

	void showPFState();

	void draw();

	void drawPfState();

	void drawGroundTruth();

	void drawMap();


private:
	CDisplayWindowPlots* pWinPF;

	CBearingOnlyParticleFilter* pParticleFilter;
	CParticleFilter	PF;

	int numParticles;

	CParticleVehicleData initialVehicleData;
	CParticleVehicleData stdDev;

	LandmarkMap landmarkMap;

	Camera1DInternal camInternal;

	float deltaTime;


	float time;
	CParticleVehicleData groundTruthVehicle;
	CObservationImageLandmarks groundTruthObservation;
	CObservationImageLandmarksPtr noisyObservationPtr;
};
